from ctypes import wintypes

import gradio as gr
import ctypes

class PositionController:
    def __init__(self):
        self.position = 0
        self.motor_dll = ctypes.CDLL('./libs/CH375DLL64.dll')
        self.motor_dll.CH375WriteData.argtypes = [ctypes.c_int32, ctypes.POINTER(ctypes.c_uint8),
                                             ctypes.POINTER(ctypes.c_int32)]
        self.motor_dll.CH375WriteData.restype = ctypes.c_int32
        self.motor_dll.CH375ReadData.argtypes = [ctypes.c_uint32, ctypes.POINTER(ctypes.c_uint8),
                                            ctypes.POINTER(ctypes.c_uint32)]
        self.motor_dll.CH375ReadData.restype = ctypes.c_uint8

        self.motor_dll.CH375OpenDevice.argtypes = [wintypes.ULONG]
        self.motor_dll.CH375OpenDevice.restype = wintypes.HANDLE

        self.motor_dll.CH375CloseDevice.argtypes = [wintypes.ULONG]
        self.motor_dll.CH375CloseDevice.restype = None

        self.motor_dll.CH375GetDeviceName.argtypes = [wintypes.ULONG]
        self.motor_dll.CH375GetDeviceName.restype = ctypes.c_void_p

        # BOOL WINAPI CH375SetTimeout(ULONG iIndex, ULONG iWriteTimeout, ULONG iReadTimeout);
        self.motor_dll.CH375SetTimeout.argtypes = [wintypes.ULONG, wintypes.ULONG, wintypes.ULONG]
        self.motor_dll.CH375SetTimeout.restype = wintypes.BOOL

        self.vel = 500

    # 封装CH375SetTimeout函数
    def set_ch375_timeout(self, index, write_timeout, read_timeout):
        success = self.motor_dll.CH375SetTimeout(index, write_timeout, read_timeout)
        if not success:
            raise ctypes.WinError()  # 如果返回FALSE，表示出错
        return True

    # 封装CH375GetDeviceName函数
    def get_ch375_device_name(self, index):
        device_name_ptr = self.motor_dll.CH375GetDeviceName(index)
        if not device_name_ptr:
            raise ctypes.WinError()  # 如果返回NULL，表示出错
        # 从指针转换为可读的字符串
        device_name = ctypes.c_char_p(device_name_ptr).value.decode('utf-8')
        return device_name

    # 封装CH375OpenDevice函数
    def open_ch375_device(self, index):
        handle = self.motor_dll.CH375OpenDevice(index)
        if handle == 0:
            raise ctypes.WinError()  # 如果返回0，表示出错
        return handle

    # 封装CH375CloseDevice函数
    def close_ch375_device(self, index):
        self.motor_dll.CH375CloseDevice(index)
        # 由于返回类型是VOID，不需要返回值

    def forward(self, step):
        self.position += step
        self.initialize_controller()
        data_list = []
        data_list.append(int('11100011',2)) #0 11100011
        data_list.append(0) #1
        data_list.append(0) #2
        data_list.append(0) #3
        data_list.append(200) #4
        data_list.append(2) #5
        data_list.append(208) #6
        data_list.append(0) #7
        data_list.append(100) #8
        bin_step = bin(step)[2:].zfill(32)
        data_list.append(int(bin_step[0:8],2))  # 9
        data_list.append(int(bin_step[8:16],2))  # 10
        data_list.append(int(bin_step[16:24],2))  # 11
        data_list.append(int(bin_step[24:32],2))  # 12
        for _ in range(20):# 13-32
            data_list.append(0)
        data_list.append(1) #33
        data_list.append(170) #34
        # i = 0
        # for data in data_list:
        #     print(f"{data}:{i}")
        #     i = i+1
        self._write_to_motor(0,data_list)
        self.close_ch375_device(0)
        return self.position

    def backward(self, step):
        self.position -= step
        self.initialize_controller()
        data_list = []
        data_list.append(int('11100010', 2))  # 0 11100010
        data_list.append(0)  # 1
        data_list.append(0)  # 2
        data_list.append(0)  # 3
        data_list.append(200)  # 4
        data_list.append(2)  # 5
        data_list.append(208)  # 6
        data_list.append(0)  # 7
        data_list.append(100)  # 8
        bin_step = bin(step)[2:].zfill(32)
        data_list.append(int(bin_step[0:8], 2))  # 9
        data_list.append(int(bin_step[8:16], 2))  # 10
        data_list.append(int(bin_step[16:24], 2))  # 11
        data_list.append(int(bin_step[24:32], 2))  # 12
        for _ in range(20):  # 13-32
            data_list.append(0)
        data_list.append(1)  # 33
        data_list.append(170)  # 34
        self._write_to_motor(0, data_list)
        self.close_ch375_device(0)
        return self.position

    def set_zero(self):
        self.position = 0
        return self.position

    def move_to_zero(self):
        step = -self.position
        if step > 0:  # 需要往前才能归零
            self.forward(step)
        if step == 0:
            pass
        if step <0:
            self.backward(-step)
        self.position = 0
        return self.position

    def initialize_controller(self):
        self.open_ch375_device(0)
        # print(self.get_ch375_device_name(0))
        self.set_ch375_timeout(0,1000,1000)
        data_list = []
        for _ in range(34):
            data_list.append(0)
        # 34-39              35 36 37 38 39
        data_list.extend([85,4,217,4,217,0])
        self._write_to_motor(0, data_list)
        return "Controller Initialized"

    def _write_to_motor(self, index, data: list[int]):
        oBuffer = (ctypes.c_uint8 * len(data))(*data)  # 创建 ctypes 数组
        ioLength = ctypes.c_int32(len(data))
        result = self.motor_dll.CH375WriteData(index, oBuffer, ctypes.byref(ioLength))
        if result != 1:
            raise IOError("Failed to write to motor.")

    def read_from_motor(self, index):
        buffer = ctypes.pointer(ctypes.c_uint8(25))  # Create a buffer for the data
        ioLength = ctypes.c_uint32(25)  # Length of the data to read
        result = self.motor_dll.CH375ReadData(index, buffer, ctypes.byref(ioLength))
        if result == 1:
            data = list(buffer)[:ioLength.value]
            print(f"Read from motor: Index={index}, Buffer={data}, Length={ioLength.value}")
            return data
        else:
            raise IOError("Failed to read from motor.")

controller = PositionController()

def update_position(action, step):
    if action == "forward":
        return controller.forward(step)
    elif action == "backward":
        return controller.backward(step)
    elif action == "set_zero":
        return controller.set_zero()
    elif action == "move_to_zero":
        return controller.move_to_zero()

# def initialize_controller():
#     return controller.initialize_controller()
#
# def close_connect():
#     return controller.close_ch375_device(0)

with gr.Blocks() as demo:
    gr.Markdown("# Position Controller")

    step_input = gr.Number(value=1, label="Step Length")

    position_output = gr.Textbox(value="0", label="Current Position", interactive=False)

    with gr.Row():
        forward_btn = gr.Button("Forward")
        backward_btn = gr.Button("Backward")
        set_zero_btn = gr.Button("Set to Zero")
        move_to_zero_btn = gr.Button("Move to Zero")
        #initialize_btn = gr.Button("Initialize Controller")
        #close_btn = gr.Button("Close Connect")

    forward_btn.click(fn=update_position, inputs=[gr.Text(value="forward", visible=False), step_input], outputs=position_output)
    backward_btn.click(fn=update_position, inputs=[gr.Text(value="backward", visible=False), step_input], outputs=position_output)
    set_zero_btn.click(fn=update_position, inputs=[gr.Text(value="set_zero", visible=False), step_input], outputs=position_output)
    move_to_zero_btn.click(fn=update_position, inputs=[gr.Text(value="move_to_zero", visible=False), step_input], outputs=position_output)
    #initialize_btn.click(fn=initialize_controller, inputs=[], outputs=position_output)
    #close_btn.click(fn=close_connect, inputs=[], outputs=[])

demo.launch()